#include <Servo.h>
#include <robotLac.h>
char INBYTE; //Entrada del bluetooth
robotLac robot(28,1);
Distance_Sensor dSensor(31, 29);
int llego=0;

void Seguirparedder();
void Seguirparedizq();

void setup(){
   Serial.begin(9600);
  robot.begin();
  robot.detener();
  dSensor.begin();
}

void loop (){  
    
    llego=0;
     if(Serial.available()){
    INBYTE = Serial.read();
    Serial.println(INBYTE);
    switch (INBYTE){
      case '1': Seguirparedder(); break;
      case '2': Seguirparedizq(); break;
      case '3': robot.detener(); break;
      
      delay(1000);
    }
  }
  
  

}



void Seguirparedder(){
while(!Serial.available()){
 if((dSensor.get_distance()>0.20)&&(llego==0)){
    robot.avanzar();
    delay(100);
    }
  else if(dSensor.get_distance()>0.20){
    llego=1;
    robot.antihorario();
    delay(200);
    robot.avanzar();
    delay(200);
  }
  else{
    llego=1;
    delay(200);
    robot.derecha();
    delay(200);
    
    if(dSensor.get_distance()<0.10){
    robot.horario();
    delay(200);
    robot.retroceder();
    delay(200);
  }
  }  }
}




void Seguirparedizq(){
while(!Serial.available()){
 if((dSensor.get_distance()>0.20)&&(llego==0)){
    robot.avanzar();
    delay(100);
    }
  else if(dSensor.get_distance()>0.20){
    llego=1;
    robot.horario();
    delay(200);
    robot.avanzar();
    delay(200);
  }
  else{
    llego=1;
    delay(200);
    robot.izquierda();
    delay(100);
    
    if(dSensor.get_distance()<0.10){
    robot.antihorario();
    delay(200);
    robot.retroceder();
    delay(200);
  }
  }  
}}
